#include "pid.h"

#define ABS(x)  ((x>0)? (x): (-x)) 


void bilateral_limit(float *a, float limit)
{
    if (limit <= 1e-4) return; // 如果限制值小于等于0，则不进行限制
    if(*a > limit) *a = limit;
    if(*a < - limit) *a = - limit;
}

/**
 * @brief PID计算函数
 * @param pid PID控制器结构体指针
 * @param feedback 当前反馈值
 * @param goal 目标值
 * @return 返回PID计算后的输出值
 */
float PID_Calculate(PID_TypeDef* pid, float feedback, float goal)
{
    pid->error[NOW] = goal - feedback;

	if (pid->deadband > 1e-4 && ABS(pid->error[NOW]) < pid->deadband)
		pid->error[NOW] = 0.0f; // 死区大于零有效，死区内不计算PID
    
    if(pid->mode == POSITION_PID) //位置式p
    {
        pid->Pout = pid->Kp * pid->error[NOW];
        pid->Iout += pid->Ki * pid->error[NOW];
        pid->Dout = pid->Kd * (pid->error[NOW] - pid->error[LAST] );
        pid->pos_out = pid->Pout + pid->Iout + pid->Dout; // 先试算总输出

        // 判断总输出是否饱和
        if (pid->pos_out > pid->output_limit) 
        {
            // 如果输出饱和，并且误差是正的（意味着还需要继续正向积分），则停止积分
            if (pid->error[NOW] > 0.001f) 
            {
                // 保持Iout不变，不进行 pid->Iout += pid->Ki * pid->error[NOW];
            }
            pid->pos_out = pid->output_limit; // 输出限幅
        } 
        else if (pid->pos_out < -pid->output_limit) 
        {
            // 如果输出饱和，并且误差是负的（意味着还需要继续反向积分），则停止积分
            if (pid->error[NOW] < -0.001f) 
            {
                // 保持Iout不变
            }
            pid->pos_out = -pid->output_limit; // 输出限幅
        }
        else
        {
            // 只有在输出不饱和时，才进行积分累加
            pid->Iout += pid->Ki * pid->error[NOW];
            bilateral_limit(&(pid->Iout), pid->integral_limit); // 仍然可以保留积分项本身的限幅
        }

        pid->error[LAST] = pid->error[NOW];
        return pid->pos_out;
    }
    if(pid->mode == DELTA_PID)//增量式P
    {
        pid->Pout = pid->Kp * (pid->error[NOW] - pid->error[LAST]);
        pid->Iout = pid->Ki * pid->error[NOW];
        pid->Dout = pid->Kd * (pid->error[NOW] - 2*pid->error[LAST] + pid->error[LLAST]);
        
        bilateral_limit(&(pid->Iout), pid->integral_limit);
        pid->delta_out[NOW] = pid->delta_out[LAST] + (pid->Pout + pid->Iout + pid->Dout);
        bilateral_limit(&(pid->delta_out[NOW]), pid->output_limit);

        pid->delta_out[LAST] = pid->delta_out[NOW];	//update last time
        pid->error[LLAST] = pid->error[LAST];
        pid->error[LAST] = pid->error[NOW];

        return pid->delta_out[NOW];
    }
    
    return 0.0f; // 如果模式不匹配，返回0
}